import matplotlib.pyplot as plt
import numpy as np
import math

# 初始化------------------------------------------------------------------------------
delta_T = 0.1  # 采样时间
T = 10  # 总时间
t_0 = np.arange(0, T, 0.01)  # 未采样前连续时间
Xk = [0] * len(t_0)
for i in range(0, len(t_0)):
    Xk[i] = t_0[i] + 3
K = list(range(1, int(T / delta_T) + 1))  # 采样次数
t = [0] * len(K)
wk = list(range(0, int(T / delta_T)))  # 干扰值
X_measure = list(range(0, int(T / delta_T)))
xhat = [0] * len(K)
xhat_pre = 0  # 设定初始的估计值
duibi = [0] * len(K)
xhat_d = [0] * len(K)  # 估计值的导数
xhat_d_pre = 0  # 设定初始的估计值
K1 = [0.0] * len(K)
K2 = [0.0] * len(K)  # K1,K2都是滤波器的增益
e = [0] * len(K)  # 误差值
# 计算测量值------------------------------------------------------------------------------
for i in range(0, len(K)):
    t[i] = (K[i] - 1) * delta_T
for i in range(0, len(K)):
    wk[i] = np.random.normal(loc=0.0, scale=5.0, size=None)
    X_measure[i] = (t[i] + 3) + wk[i]

# 1阶滤波器估计值的计算-------------------------------------------------------------------
m1 = np.array([[len(K), sum(t)], [sum(t), sum(list(map(lambda x: x ** 2, t)))]])
# 最后一个求和符号里面是求数列个元素平方
w = [t * X_measure for t, X_measure in zip(t, X_measure)]
m2 = np.array([[sum(X_measure)], [sum(w)]])
a = (np.linalg.inv(m1)).dot(m2)  # m1求逆再乘以m2
a0 = float(a[0])
a1 = float(a[1])

for i in range(0, len(K)):
    t[i] = (K[i] - 1) * delta_T
    duibi[i] = a0 + a1 * t[i]
# 1阶迭代最小二乘滤波器----------------------------------------------------------------
for i in range(0, len(K)):
    K1[i] = (2 * (2 * K[i] - 1)) / (K[i] * (K[i] + 1))
    K2[i] = 6 / (K[i] * (K[i] + 1) * delta_T)
    RESk = X_measure[i] - xhat_pre - (xhat_d_pre * delta_T)
    xhat[i] = xhat_pre + xhat_d_pre * delta_T + K1[i] * RESk
    xhat_d[i] = xhat_d_pre + K2[i] * RESk
    xhat_pre = xhat[i]
    xhat_d_pre = xhat_d[i]
# 计算估计值与真实值的误差----------------------------------------------------------
for i in range(0, len(K)):
    e[i] = xhat[i] - (t[i] + 3)
# 画图-----------------------------------------------------------------------------------
plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t, X_measure, '-o', label='measures')
plt.plot(t, xhat, label='first-order R least squares filters')
plt.plot(t, duibi, '--', label='first-order least squares filters')
plt.legend()  # 打上标签
plt.show()

plt.xlabel("Time(Sec)")
plt.ylabel("error between estimates ang true signal")
plt.plot(t, e, label='error between estimates ang true signal')
plt.grid()
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
plt.ylabel("x Dot")
plt.axhline(y=0, c='k', lw=2, label='the derivative of true signal')
plt.plot(t, xhat_d, '--', label='the derivative of estimates')
plt.legend()
plt.show()
